clc; clear; close all;
load trjPole10ms.mat;
glvs;

nn = 2;
imu = trj.imu;
len = length(imu);
nav = GridNav(trj.avp0, trj.ts*nn);
ins_avp = zeros(len,10);
timebar(nn,len);
ks = 1;
for k=nn:nn:len
    k0 = k-nn+1;
    wm = imu(k0:k,1:3);
    vm = imu(k0:k,4:6);
    nav.update(wm, vm);
%     nav.update(wm, vm, trj.avp(k,7:9)', trj.avp(k,4:6)');
    ins_avp(ks,:) = [nav.avp; k*trj.ts]';
    ks = ks+1;
    timebar;
end

ins_avp = ins_avp(1:ks-1,:);
insplot(ins_avp);
avperr = avpcmpplot(trj.avp, ins_avp);


